#include "stdafx.h"
#include "hm.h"

namespace cvLib
{
	void cvLib::HM::loadPeopleInfo
		(
		Cube<eT>&				dataLoad, 
		int&					szSeq, 
		int&					szPeople
		)
	{
		std::ostringstream convertS;	
		arma::Mat<eT> dataInfo;

		convertS.str("..\\data\\dataInfo"); 
		dataInfo.load(convertS.str(),raw_ascii);

		szPeople = int(dataInfo.at(0,0));
		szSeq = int(dataInfo.at(0,1));

		dataLoad.set_size(szSeq,3,szPeople);	

		for(int i=0;i<szPeople;i++)
		{
			convertS.str(""); 
			convertS << "..\\data\\data_" << i ;
			dataLoad.slice(i).load(convertS.str(),raw_ascii);		
		}
	}
	
	void cvLib::HM::heatMapCalc
		(
		Col<eT>&				locX, 
		Col<eT>&				locY, 
		Col<eT>&				locOri,
		const HMPara&			p
		)	
	{
		int szDataPeople = this->szCurPeopleHM;
		int frameNo = 0;
		// Initialize        
		iFoINum.zeros();
		RoomS.zeros();  

		// Update New information
		//updatePeopleInfo(frameNo,dataLoad,szGround,szGround,locX,dataLocY,dataLocOri,dataSzPeople);

		// Validate Information
		validatePeopleInfo(locX,locY,locOri,szDataPeople,para);

		for(int iCntPeople=0;iCntPeople<szDataPeople;iCntPeople++)
		{
			int personLocX = (int)(locX[iCntPeople]);
			int personLocY = (int)(locY[iCntPeople]);
			int personOri = (int)(locOri[iCntPeople]);

			if( (personLocX ==0) && (personLocY ==0) && (personOri ==0))
			{
				personRoICube.slice(iCntPeople).zeros();
				continue; // invalid data
			}

			// Add PersonROI into groundTemplate
			groundTemplate.zeros();            

			//groundTemplate.submat(personLocX,personLocY,personLocX+sizeROIMatrix-1,personLocY+sizeROIMatrix-1) = personROICubeTemplate.slice(personOri);              
			convertS.str(""); convertS << "..\\ROITemplate\\" << personOri ;	RoILoad.load(convertS.str(),arma_binary);
			groundTemplate.submat(personLocY,personLocX,personLocY+p.szROIMatrix-1,personLocX+p.szROIMatrix-1) =  RoILoad;

			// Crop the RoomS                      
			personRoICube.slice(iCntPeople) = groundTemplate.submat(p.szRadiusROI+1,p.szRadiusROI+1,p.szRadiusROI+p.szGround,p.szRadiusROI+p.szGround);
			RoomS += personRoICube.slice(iCntPeople); 
			//personST.slice(iCntPeople) = personRoICube.slice(iCntPeople); 			

			iFoINum += (personRoICube.slice(iCntPeople)> zeroGround);   			
		}

		iFoIMaskMulti = iFoINum > iFoIThres;
		iFoIMaskDiv = iFoINum + iFoIThres - iFoIMaskMulti;
		RoomS = (RoomS%iFoIMaskMulti) / iFoIMaskDiv;        

		RoomST = RoomST *p.decayCoef + RoomS;
		personST = personST * p.decayCoef + personRoICube;

		interactionCenterCnt = 0;
		//if(frameNo>=302)
		{
			//Interaction Center Seeking
			interactionCenterSeeking
				(				
				interactionCenterX, interactionCenterY, interactionCenterCnt,
				RoomST,
				para
				);
		}	

		int RealICcnt = interactionCenterCnt;
		int ICptr = 0;

		arma::Col<int> interactantIdx(MaxNumPeople);	
		arma::Col<int> centerInteractantCnt;
		arma::Mat<int> centerInteractantRes;

		//Interactant Detection
		if(interactionCenterCnt >0)
		{
			interactantIdx.fill(-1);			

			interactionValue.zeros();
			for(int iCenter = 0;iCenter<interactionCenterCnt;iCenter++)
			{
				for(int iPeople=0;iPeople<szDataPeople;iPeople++)
				{
					interactionValue.at(iCenter,iPeople) = getInteractionValue(	personST.slice(iPeople),
						/*szGround,
						szGround,*/
						interactionCenterY.at(iCenter),
						interactionCenterX.at(iCenter),
						interactantCircleMask,
						para
						/*interactantRadius*/)
						*p.normValue;
					if(interactionValue.at(iCenter,iPeople) > p.interactantThres)
					{
						if(interactantIdx.at(iPeople)<0 || interactionValue.at(iCenter,iPeople) > interactionValue.at(interactantIdx.at(iPeople),iPeople))
							interactantIdx.at(iPeople) = iCenter; 
					}					
				}
			}

			centerInteractantCnt.set_size(interactionCenterCnt);
			centerInteractantRes.set_size(interactionCenterCnt,MaxNumPeople);

			centerInteractantCnt.zeros();
			centerInteractantRes.zeros();

			for(int iPeople=0;iPeople<szDataPeople;iPeople++)
			{
				int tmpCenterNo = interactantIdx.at(iPeople);
				if (tmpCenterNo>=0)
				{
					centerInteractantRes.at(tmpCenterNo,iPeople) = 1;
					centerInteractantCnt.at(tmpCenterNo)++;
				}
			}

			for(int iCenter = 0;iCenter<interactionCenterCnt;iCenter++)
			{
				if ( centerInteractantCnt.at(iCenter) < 2) // less than two persons in the interaction
				{
					centerInteractantRes.row(iCenter).fill(0);
					centerInteractantCnt.at(iCenter) = 0;
				}				
				else
				{
					//resInteractant.slice(frameNo).row(ICptr)= centerInteractantRes.row(iCenter); // 0 1 0 1 0 1			
					//resCenterLoc.slice(frameNo).at(ICptr,0) = interactionCenterX.at(iCenter);
					//resCenterLoc.slice(frameNo).at(ICptr++,1) = interactionCenterY.at(iCenter);
				}
			}

			//resCenterNo.at(frameNo) = ICptr;		
		}

		//Best View Selection
		if(ICptr >0)
		{
			for(int iCenter = 0;iCenter<interactionCenterCnt;iCenter++)
			{
				eT xC, yC;
				xC = eT(interactionCenterX.at(iCenter));
				yC = eT(interactionCenterY.at(iCenter));

				if(centerInteractantCnt.at(iCenter) > 0)
				{
					RoomBV.zeros();
					eT maxDisS = -1;
					eT tmpX, tmpY, tmpDisS;
					//Step1: find the furthest person
					for(int iPeople=0;iPeople<szDataPeople;iPeople++)
					{
						if (centerInteractantRes.at(iCenter,iPeople)>0)
						{
							tmpX = eT(locX.at(iPeople));
							tmpY = eT(locY.at(iPeople));
							tmpDisS = (xC-tmpX)*(xC-tmpX) + (yC-tmpY)*(yC-tmpY);
							if (tmpDisS>maxDisS)
							{
								maxDisS = tmpDisS;
							}
							RoomBV = RoomBV + personST.slice(iPeople);
						}
					}

					int bestR = int(floor(sqrt(maxDisS)));

					/*if(bestR > 250 )
					printf("why");*/


					/*void getBestViewValue(const Mat<eT>& heatMap, const int& sizeRow, const int& sizeCol, 
					const int& locX, const int& locY, const int& innerR, const int& deltaR,
					const Cube<eT> sectorMaskTemplate, const int& sMN, const int& sMRadius,
					Col<eT>& bValue
					)*/
					//Step2: create mask to get the value of each area
					getBestViewValue
						(
						bValue,
						RoomBV,
						int(xC),
						int(yC), 
						sectorMaskTemplate,
						bestR,
						para
						); 
				}
			}
		}
	}


	void cvLib::HM::updatePeopleInfo
		(
		const int&				frameNo, 
		const Cube<eT>&			dataLoad,	//input
		const int&				szXLim, 
		const int&				szYLim, // check validity
		Col<int>&				locX,
		Col<int>&				locY, 
		Col<int>&				locOri,
		int&					cntPeople
		)	//output
	{
		cntPeople = dataLoad.n_slices;
		for(int i=0;i<cntPeople;i++)
		{
			locX[i] = (int) (dataLoad.slice(i).at(frameNo,0));
			locY[i] = (int)(dataLoad.slice(i).at(frameNo,1));
			locOri[i] = (int)(dataLoad.slice(i).at(frameNo,2));

			if(locX[i] >= szXLim || locX[i]<0 || locY[i] >= szYLim || locY[i]<0)	// check data validity
			{
				locX[i]=0;
				locY[i]=0;
				locOri[i] = 0;
			}
			locOri[i] = (locOri[i]+360)%360; // Assume the Orientation is in range [-360,360]
		}
	};

	void HM::validatePeopleInfo
		(
		Col<eT>&				locX,
		Col<eT>&				locY, 
		Col<eT>&				locOri,
		const int&				cntPeople,		
		const HMPara&			p
		)
	{
		const int szXLim = p.szGround;
		const int szYLim = p.szGround;

		int tmpOri;
		for(int i=0;i<cntPeople;i++)
		{		
			if(locX[i] >= szXLim || locX[i]<0 || locY[i] >= szYLim || locY[i]<0)	// check data validity
			{
				locX[i]=0;
				locY[i]=0;
				locOri[i] = 0;
			}
			tmpOri = (int)locOri[i];
			locOri[i] = (tmpOri+360)%360; // Assume the Orientation is in range [-360,360]
		}
	}

	void HM::personROIMask
		(
		arma::Mat<eT>&			personROIMatrix,
		const eT&				ori,
		const eT&				radius,
		const HMPara&			p
		)

		//const eT& locX, const eT& locY, const eT& ori, const eT& deltaOri, const eT& radius, )
		/************************************************************************/
		/*  Create the template for Region of Interest
		/*  locX, locY, ori --> spatial coordinate and orientation of people
		/*  2*deltaOri, radius --> angle of RoI, radius of RoI	
		/************************************************************************/
	{				
		const eT angleS = ori - p.deltaOri;
		const eT angleE = ori + p.deltaOri;	
		const eT sectorStartX = cos(angleS*PI/180);
		const eT sectorStartY = sin(angleS*PI/180);
		const eT sectorEndX = cos(angleE*PI/180);
		const eT sectorEndY = sin(angleE*PI/180);
		const eT squareRadius = eT(p.szRadiusROI*p.szRadiusROI);

		// Create Mask
		personROIMatrix.zeros();
		int dimRow = personROIMatrix.n_rows;
		int dimCol = personROIMatrix.n_cols;

		for(int i = 0;i < dimRow; i++)
			for (int j = 0;j < dimCol; j++)
			{
				personROIMatrix.at(i,j) = isPointInCircularSector((eT)(j+1),(eT)(i+1),p.szRadiusROI+1,p.szRadiusROI+1,sectorStartX,sectorStartY,sectorEndX,sectorEndY,squareRadius);			
			}
	}

	bool HM::isPointInCircularSector
		(
		const eT&				pX, 
		const eT&				pY, 
		const eT&				cX, 
		const eT&				cY,
		const eT&				sectorSX, 
		const eT&				sectorSY, 
		const eT&				sectorEX, 
		const eT&				sectorEY, 
		const eT&				squareRadius
		)
	{	
		eT relPX = pX - cX;
		eT relPY = pY - cY;
		eT squareDis = relPX*relPX + relPY*relPY;
		if (fabs(squareDis-0)< EPS) return true; 	
		eT numDiv = sqrt(squareDis);

		relPX = relPX/numDiv;
		relPY = relPY/numDiv;

		return ( 
			(squareDis<=squareRadius) &&
			areClockwise (relPX,relPY,sectorSX,sectorSY) &&
			areClockwise (sectorEX,sectorEY,relPX,relPY)
			);		   
	}

	arma::Mat<eT> HM::getGaussianMatrix
		(
		const HMPara&			p	
		)
	{
		const eT		centerX		= p.szROIMatrix+1; 
		const eT		centerY		= p.szROIMatrix+1;
		const int		sizeMatrix	= p.szROIMatrix*2+1;

		arma::Mat<eT>	gaussM(sizeMatrix,sizeMatrix);

		// (j+1)--> x; (i+1) --> y;
		for(int i = 0;i < sizeMatrix; i++) 
			for(int j = 0;j < sizeMatrix;j++)
			{
				gaussM.at(i,j) = exp( 
					-((j+1)-centerX)*((j+1)-centerX)/(2*p.sigmaXSquare) -
					((i+1)-centerY)*((i+1)-centerY)/(2*p.sigmaYSquare) 
					);
			}
			return gaussM;
	}

	void HM::getBestViewValue
		(
		arma::Col<eT>&			retBValue,
		const arma::Mat<eT>&	heatMap, 
		const int&				locX, 
		const int&				locY, 
		const Cube<eT>&			sectorMaskTemplate, 
		const int&				innerR,
		const HMPara&			p
		)
	{
		int sizeRow			= p.szGround;
		int sizeCol			= p.szGround;		

		// 1. heatMap --crop--> copyHeatMap	
		const int radius =  innerR+p.deltaR; 
		arma::Mat<eT> copyHeatMap(2*radius+1,2*radius+1);
		int rowU, rowD, colL, colR;
		int subrowU, subrowD, subcolL, subcolR;

		rowU = MAXNUM(locY - radius - 1,0);
		rowD = MINNUM(locY + radius - 1,sizeRow-1);
		colL = MAXNUM(locX - radius - 1,0);
		colR = MINNUM(locX + radius - 1,sizeCol-1);

		subrowU = MAXNUM(radius - locY - 1,0);
		subrowD = subrowU + (rowD-rowU);		
		subcolL = MAXNUM(radius - locX - 1,0);		
		subcolR = subcolL + (colR-colL);	

		copyHeatMap.zeros();
		copyHeatMap.submat(subrowU,subcolL,subrowD,subcolR) = heatMap.submat(rowU,colL,rowD,colR);	

		// 2. Create Ring Mask
		arma::Mat<eT> ringMask(2*radius+1,2*radius+1);

		ringMask.ones();
		int boundR = 2*radius+1;
		int boundRInnerSquare = innerR*innerR;
		int boundROuterSquare = radius*radius;
		int tmpDis;
		for(int i=0;i<boundR;i++)
		{
			for(int j=0;j<boundR;j++)
			{
				tmpDis = ((i-radius)*(i-radius) + (j-radius)*(j-radius));
				if( tmpDis > boundRInnerSquare && tmpDis < boundROuterSquare)
					ringMask.at(i,j) = 1;				
			}
		}

		arma::Mat<eT> sectorMask(2*radius+1,2*radius+1);
		arma::Mat<eT> tmpMatrix(2*radius+1,2*radius+1);
		eT maskArea = PI*(radius*radius - innerR*innerR)/p.bViewMaskN;

		// 3. Crop Sector Mask
		for(int iSMN=0;iSMN<p.bViewMaskN;iSMN++)
		{
			sectorMask = sectorMaskTemplate.slice(iSMN).submat(p.bViewMaskMaxR - radius,p.bViewMaskMaxR - radius, p.bViewMaskMaxR + radius,p.bViewMaskMaxR + radius);

			// 4. Combine all of them
			tmpMatrix = copyHeatMap % ringMask % sectorMask;

			/*tmpMatrix.print("combine");*/
			retBValue.at(iSMN) = accu(tmpMatrix)/maskArea;
		}	
	}

	bool HM::convolve2D
		(
		const arma::Mat<eT>&	in, 
		arma::Mat<eT>& out, 
		const int&				sizeRow, 
		const int&				sizeCol, 
		const arma::Mat<eT>&	ker, 
		const int&				kernelSizeX, 
		const int&				kernelSizeY
		)
	{
		int i, j, m, n;    
		int kCenterX, kCenterY;
		int mm,nn,ii,jj;

		// find center position of kernel (half of kernel size)
		kCenterX = kernelSizeX >> 1;
		kCenterY = kernelSizeY >> 1;    

		// start convolution
		for(i= 0; i < sizeRow; ++i)                   // number of rows
			for(j = 0; j < sizeCol; ++j)              // number of columns
			{
				out.at(i,j) = 0;                            // set to 0 before accumulate

				if(fabs(in.at(i,j)-0.0f)<=EPS) continue;
				// flip the kernel and traverse all the kernel values
				// multiply each kernel value with underlying input data
				for(m = 0; m < kernelSizeY; ++m)        // kernel rows
				{
					mm = kernelSizeY - 1 - m;

					for(n = 0; n < kernelSizeX; ++n)
					{
						nn = kernelSizeX - 1 - n;

						ii = i+(m-kCenterY);
						jj = j+(n-kCenterX);

						if(ii>=0 && ii<sizeRow && jj>=0 && jj<sizeCol)
							out.at(i,j) += in.at(ii,jj) * ker.at(mm,nn);
					}
				}            
			}
			return true;    
	}

	bool HM::convolveSep
		(		
		arma::Mat<eT>&			resOut, 		
		const arma::Mat<eT>&	in, 
		const arma::Col<eT>&	ker,
		const HMPara			p				
		)
	{		
		int sizeRow		=	p.szGround;
		int sizeCol		=	p.szGround;
		int kernelSize	=	p.szFilter;

		arma::Mat<eT> tmp(sizeRow,sizeCol);	
		arma::Col<eT> sum(sizeRow);	

		// find center position of kernel (half of kernel size)
		int kCenter		= kernelSize >> 1;                          // center index of kernel array
		int endIndex	= sizeRow - kCenter;                 // index for full kernel convolution
		int kOffset;                 // kernel indice
		int inPtr		= 0;
		int tmpPtr		= 0; 

		tmp.zeros();
		// start horizontal convolution (x-direction)
		for(int i = 0; i < sizeRow; ++i)                    // number of rows
		{
			kOffset = 0;                                // starting index of partial kernel varies for each sample
			// COLUMN FROM index=0 TO index=kCenter-1
			for(int j = 0; j < kCenter; ++j)
			{						
				if(fabs(in.at(i,j) - 0) > EPS )
				{
					for(int k = kCenter + kOffset, m = 0; k >= 0; --k, ++m) // convolve with partial of kernel
					{
						tmp.at(i,j) += in.at(i,m) * ker.at(k);
					}			
				}
				++kOffset;                              // increase starting index of kernel
			}

			// COLUMN FROM index=kCenter TO index=(dataSizeX-kCenter-1)
			for(int j = kCenter, jj = 0; j < endIndex; ++j,++jj)
			{			
				if( fabs(in.at(i,j) - 0) > EPS )
				{
					for(int k = kernelSize-1, m = 0; k >= 0; --k, ++m)  // full kernel
					{
						tmp.at(i,j) += in.at(i,m+j-kCenter) * ker.at(k);
					}
				}
			}

			kOffset = 1;                                // ending index of partial kernel varies for each sample
			// COLUMN FROM index=(dataSizeX-kCenter) TO index=(dataSizeX-1)
			for(int j = endIndex, jj = 0; j < sizeCol; ++j,++jj)
			{			
				if( fabs(in.at(i,j) - 0) > EPS )
				{
					for(int k = kernelSize-1,m = 0; k >= kOffset; --k, ++m)   // convolve with partial of kernel
					{
						tmp.at(i,j) += in.at(i,m+endIndex-kCenter+jj) * ker.at(k);
					}
				}
				++kOffset;                              // increase ending index of partial kernel
			}		
		}
		// END OF HORIZONTAL CONVOLUTION //////////////////////

		/*tmp.print("tmp");
		tmp = in;*/

		// start vertical direction ///////////////////////////
		// find center position of kernel (half of kernel size)
		kCenter = kernelSize >> 1;                          // center index of vertical kernel
		endIndex = sizeCol - kCenter;                 // index where full kernel convolution should stop

		// clear out array before accumulation
		/*for(int i = 0; i < sizeCol; ++i)
		sum[i] = 0;*/
		sum.zeros();
		// start to convolve vertical direction (y-direction)

		// ROW FROM index=0 TO index=(kCenter-1)
		kOffset = 0;                                    // starting index of partial kernel varies for each sample
		for(int i=0; i < kCenter; ++i)
		{	
			for(int j=0; j < sizeCol; ++j)
			{
				if(   fabs(tmp.at(i,j) - 0) > EPS )
				{
					for(int k = kCenter + kOffset,jj=0; k >= 0; --k,++jj)     // convolve with partial kernel
					{	
						sum.at(j) += tmp.at(jj,j) * ker.at(k);				
					}
				}
			}

			resOut.row(i) = sum.t();
			sum.zeros();
			//for(int n = 0; n < sizeCol; ++n)              // convert and copy from sum to out
			//{
			//	resOut.at(i,n) = sum.at(n);                       // store final result to output array
			//	sum.at(n) = 0;                             // reset to zero for next summing			
			//}		
			++kOffset;                                  // increase starting index of kernel
		}

		// ROW FROM index=kCenter TO index=(dataSizeY-kCenter-1)
		for(int i = kCenter; i < endIndex; ++i)
		{
			for(int j = 0; j < sizeCol; ++j)
			{
				if(   fabs(tmp.at(i,j) - 0) > EPS )
				{
					for(int k = kernelSize -1,jj=0; k >= 0; --k,++jj)             // convolve with full kernel
					{
						sum.at(j) += tmp.at(jj+i-kCenter,j) * ker.at(k);				
					}
				}
			}

			resOut.row(i) = sum.t();
			sum.zeros();
			//for(int n = 0; n < sizeCol; ++n)              // convert and copy from sum to out
			//{
			//	resOut.at(i,n) = sum.at(n);                       // store final result to output buffer
			//	sum.at(n) = 0;                             // reset before next summing			
			//}
		}

		// ROW FROM index=(dataSizeY-kCenter) TO index=(dataSizeY-1)
		kOffset = 1;                                    // ending index of partial kernel varies for each sample
		for(int i=endIndex; i < sizeRow; ++i)
		{
			for(int j=0; j < sizeCol; ++j)
			{
				if(   fabs(tmp.at(i,j) - 0) > EPS )
				{
					for(int k = kernelSize-1,jj=0; k >= kOffset; --k,++jj)        // convolve with partial kernel
					{
						sum.at(j) += tmp.at(jj+endIndex-kCenter,j) * ker.at(k);
					}
				}
			}

			resOut.row(i) = sum.t();
			sum.zeros();
			//for(int n = 0; n < sizeRow; ++n)              // convert and copy from sum to out
			//{
			//	resOut.at(i,n)= sum.at(n);                       // store final result to output array
			//	sum.at(n) = 0;                             // reset to 0 for next sum
			//	// next output
			//}		
			++kOffset;                                  // increase ending index of kernel
		}
		// END OF VERTICAL CONVOLUTION ////////////////////////
		return true;
	}

	void HM::smoothImage
		(
		arma::Mat<eT>& A, 
		const HMPara& p		
		)
	{
		arma::Mat<eT> Acopy = A;

		/* Naive Convolution 2D */
		/*Mat<eT> kernel(szKernel,szKernel);    
		int kernelSizeX = szKernel;
		int kernelSizeY = szKernel;
		kernel.fill(1.0f/(szKernel*szKernel));
		convolve2D(Acopy, A, sizeRow, sizeCol, kernel, kernelSizeX, kernelSizeY);   */

		/* Separable Convolution 2D */
		Col<eT> kernel(p.szFilter);    	
		kernel.fill(1.0f/p.szFilter);	
		convolveSep(A ,Acopy, kernel,p); 		
	}

	void HM::interactionCenterSeeking
		(		
		arma::Col<int>&			retX, 
		arma::Col<int>&			retY, 
		int&					retCnt,
		const arma::Mat<eT>&	hmICS, 		
		const HMPara&			p
		)								// Output
	{		
		const int sizeRow = p.szGround;
		const int sizeCol = p.szGround;
		const int edg = 5;				

		arma::Mat<eT> hmCur;
		arma::Mat<eT> peakM(sizeRow,sizeCol);

		// 1. Median Filter

		// 2. Apply threshold
		hmCur = hmICS % (hmICS>p.interactionCenterThres);  		
		if(accu(hmCur) <= EPS )	 return;
		/*cout << "Apply threshold: " << timerInteractionCenter.toc() << endl;
		timerInteractionCenter.tic();*/

		// 3. Apply Filtering
		smoothImage(hmCur,p);
		hmCur = hmCur % (hmCur> (0.9f*p.interactionCenterThres)); // Apply again threshold (and change if needed according to SNR)
		if(accu(hmCur) <= EPS )	 return;
		/*cout << "Convolution: " << timerInteractionCenter.toc() << endl;
		timerInteractionCenter.tic();*/

		// 4. Loop for peak
		peakM.zeros();
		for(int i=1;i<(sizeRow-edg-1);i++)
			for(int j=1;j<(sizeRow-edg-1);j++)
			{											
				if(
					hmCur.at(i,j) > hmCur.at(i-1,j-1) &&
					hmCur.at(i,j) > hmCur.at(i-1,j) &&
					hmCur.at(i,j) > hmCur.at(i-1,j+1) &&
					hmCur.at(i,j) > hmCur.at(i,j-1) &&
					hmCur.at(i,j) > hmCur.at(i,j+1) &&
					hmCur.at(i,j) > hmCur.at(i+1,j-1) &&
					hmCur.at(i,j) > hmCur.at(i+1,j) &&
					hmCur.at(i,j) > hmCur.at(i+1,j+1)
					)
				{
					peakM.at(i,j) = hmCur.at(i,j);					
				}                
			} 

			/*cout << "Find local maxima: " << timerInteractionCenter.toc() << endl;
			timerInteractionCenter.tic();*/

			// 5. Post Processing 
			retCnt = 0;
			bool is_true = false;
			arma::uword tmpLocRow, tmpLocCol;				
			int rowU, rowD, colL, colR;

			while(!is_true)
			{
				peakM.max(tmpLocRow, tmpLocCol);
				retX[retCnt] = (int)tmpLocCol;
				retY[retCnt++] = (int)tmpLocRow;	 // !!!gantian 

				// Merge Centers
				rowU = MAXNUM((int)tmpLocRow - p.postProcR - 1, 0        );
				rowD = MINNUM((int)tmpLocRow + p.postProcR - 1, sizeRow-1);
				colL = MAXNUM((int)tmpLocCol - p.postProcR - 1, 0        );
				colR = MINNUM((int)tmpLocCol + p.postProcR - 1, sizeCol-1);
				arma::Mat<eT> zero(rowD-rowU+1, colR-colL+1);
				zero.zeros();
				peakM.submat(rowU,colL,rowD,colR) = zero;

				if(accu(peakM) <= EPS ) is_true = true;
			}
			/*cout << "Merge Centers: " << timerInteractionCenter.toc() << "\n\n";*/
	}

	eT HM::getInteractionValue
		(
		const arma::Mat<eT>&	heatMap, 
		const int&				locRow, 
		const int&				locCol, 
		const arma::Mat<eT>&	maskMatrix, 		
		const HMPara&			p
		)
	{	
		const int sizeRow = p.szGround;
		const int sizeCol = p.szGround;
		const int r = p.interactantRadius;

		eT ret;
		int xU, xD, yL, yR;
		int subxU, subxD, subyL, subyR;

		xU = MAXNUM(locRow - r-1,0);
		xD = MINNUM(locRow + r-1,sizeRow-1);
		yL = MAXNUM(locCol - r-1,0);
		yR = MINNUM(locCol + r-1,sizeCol-1);

		subxU = MAXNUM(r-locRow-1,0);
		subxD = subxU + (xD-xU);		
		subyL = MAXNUM(r-locCol-1,0);
		subyR = subyL + (yR-yL);

		arma::Mat<eT> subHeatMap(2*r+1,2*r+1);
		subHeatMap.zeros();
		subHeatMap.submat(subxU,subyL,subxD,subyR) = heatMap.submat(xU,yL,xD,yR);

		subHeatMap = subHeatMap % maskMatrix;
		ret = accu(subHeatMap)/(PI*r*r);

		return ret;
	}
}